# servo_sweep.py
import time
from smbus2 import SMBus
import threading
import signal
import sys

# PCA9685 配置
PCA9685_ADDR = 0x40
PWM_FREQ = 50  # 舵机通常使用50Hz

class PCA9685:
    def __init__(self, address=PCA9685_ADDR, bus_number=1):
        self.address = address
        self.bus = SMBus(bus_number)
        self.setup_pwm()

    def setup_pwm(self):
        """初始化PWM设置"""
        # 重置 PCA9685
        self.bus.write_byte_data(self.address, 0x00, 0x00)

        # 设置 PWM 频率为 50Hz
        prescale = int(round(25000000.0 / 4096.0 / PWM_FREQ - 1.0))
        self.bus.write_byte_data(self.address, 0x00, 0x10)  # 睡眠模式
        self.bus.write_byte_data(self.address, 0xFE, prescale)  # 设置预分频器
        self.bus.write_byte_data(self.address, 0x00, 0x00)  # 正常运行
        time.sleep(0.005)
        self.bus.write_byte_data(self.address, 0x00, 0x80)  # 重启

    def set_pwm(self, channel, on, off):
        """设置指定通道的PWM值"""
        if channel < 0 or channel > 15:
            raise ValueError("通道必须在0-15之间")
        if on < 0 or on > 4095:
            raise ValueError("ON值必须在0-4095之间")
        if off < 0 or off > 4095:
            raise ValueError("OFF值必须在0-4095之间")

        self.bus.write_byte_data(self.address, 0x06 + 4 * channel, on & 0xFF)
        self.bus.write_byte_data(self.address, 0x07 + 4 * channel, on >> 8)
        self.bus.write_byte_data(self.address, 0x08 + 4 * channel, off & 0xFF)
        self.bus.write_byte_data(self.address, 0x09 + 4 * channel, off >> 8)

    def set_servo_angle(self, channel, angle):
        """设置舵机角度 (0-180度)"""
        # 将角度转换为PWM值
        # 50Hz时，脉冲宽度1-2ms对应205-410值
        pulse = int(205 + (angle / 180.0) * 205)
        print(f"设置通道 {channel} 脉宽 {pulse}")
        self.set_pwm(channel, 0, pulse)

class ServoController:
    def __init__(self, pca9685):
        self.pca9685 = pca9685
        self.running = True
        self.threads = []

    def servo_sweep(self, channel, min_angle=0, max_angle=180, delay=0.01):
        """单个舵机往复运动"""
        try:
            while self.running:
                # 正向运动
                for angle in range(min_angle, max_angle + 1, 2):
                    if not self.running:
                        return
                    self.pca9685.set_servo_angle(channel, angle)
                    time.sleep(delay)

                # 反向运动
                for angle in range(max_angle, min_angle - 1, -2):
                    if not self.running:
                        return
                    self.pca9685.set_servo_angle(channel, angle)
                    time.sleep(delay)
        except Exception as e:
            print(f"舵机通道 {channel} 错误: {e}")

    def start_all_servos(self):
        """启动所有16个舵机的往复运动"""
        print("启动所有舵机往复运动...")
        self.running = True

        # 为每个舵机通道创建一个线程
        for channel in range(16):
            # 为每个舵机设置不同的起始角度，避免同时启动时电流过大
            thread = threading.Thread(
                target=self.servo_sweep,
                args=(channel,),
                name=f"Servo-{channel}"
            )
            thread.daemon = True
            self.threads.append(thread)
            thread.start()
            # 稍微延迟启动，避免电流冲击
            time.sleep(0.1)

        print("所有舵机已启动")

    def stop_all_servos(self):
        """停止所有舵机运动"""
        print("停止所有舵机...")
        self.running = False

        # 等待所有线程结束
        for thread in self.threads:
            thread.join(timeout=1.0)

        # 将所有舵机设置到中间位置
        for channel in range(16):
            try:
                self.pca9685.set_servo_angle(channel, 90)
            except:
                pass

        print("所有舵机已停止并设置到中间位置")

def signal_handler(sig, frame):
    """信号处理函数，用于优雅退出"""
    print("\n收到退出信号，正在停止舵机...")
    controller.stop_all_servos()
    sys.exit(0)

def main():
    """主函数"""
    global controller

    try:
        # 初始化PCA9685
        print("初始化PCA9685...")
        pca = PCA9685()
        print("PCA9685初始化成功")

        # 创建舵机控制器
        controller = ServoController(pca)

        # 设置信号处理
        signal.signal(signal.SIGINT, signal_handler)
        signal.signal(signal.SIGTERM, signal_handler)

        # 启动所有舵机
        controller.start_all_servos()

        # 保持程序运行
        print("舵机正在运行中... 按 Ctrl+C 停止")
        while True:
            time.sleep(1)

    except Exception as e:
        print(f"程序错误: {e}")
        if 'controller' in globals():
            controller.stop_all_servos()

if __name__ == "__main__":
    main()
